#include "JY901B.h"

JY901_DataStr JY901_data;
struct SAcc_S stcAcc;
struct SGyro_S stcGyro;
struct SAngle_S stcAngle;
struct SMag_S stcMag;
struct SPress_S stcPress;
struct SLonLat_S stcLonLat;
struct SQ_S stcQ;
JY901QUEUE_S jy901data;
// 锟斤拷锟秸结构锟斤拷锟绞硷拷锟�
void JY901_USART_Init(void)
{
	for (uint16_t i = 0; i < RXBUFFER_LEN; i++)
		JY901_data.RxBuffer[i] = 0;
	JY901_data.frame_head = 0x55;
	JY901_data.Rx_flag = 0;
	JY901_data.Rx_len = 0;
	// 锟斤拷锟斤拷DMA锟斤拷锟斤拷锟叫讹拷
	__HAL_UART_ENABLE_IT(&USARTHANDLE, UART_IT_IDLE);
	HAL_UART_Receive_DMA(&USARTHANDLE, JY901_data.RxBuffer, RXBUFFER_LEN);
}

// 锟斤拷锟捷斤拷锟斤拷
void JY901_Process(void)
{

	if (JY901_data.Rx_len < RXBUFFER_LEN)
		return; // 锟斤拷锟轿伙拷锟斤拷锟斤拷锟�

	for (uint8_t i = 0; i < 9; i++)
	{
		if (JY901_data.RxBuffer[i * 11] != JY901_data.frame_head)
			return; // 锟斤拷锟街⊥凤拷锟斤拷锟�
		switch (JY901_data.RxBuffer[i * 11 + 1])
		{
		case 0x51: // 锟斤拷锟劫度斤拷锟斤拷
			memcpy(&stcAcc, &JY901_data.RxBuffer[2 + i * 11], 8);
			for (uint8_t j = 0; j < 3; j++)
				JY901_data.acc.a[j] = (float)stcAcc.a[j] / 32768 * 16;
			break;

		case 0x52: // 锟斤拷锟劫度斤拷锟斤拷
			memcpy(&stcGyro, &JY901_data.RxBuffer[2 + i * 11], 8);
			for (uint8_t j = 0; j < 3; j++)
				JY901_data.w.w[j] = (float)stcGyro.w[j] / 32768 * 2000;
			break;

		case 0x53: // 锟角度斤拷锟斤拷
			memcpy(&stcAngle, &JY901_data.RxBuffer[2 + i * 11], 8);
			for (uint8_t j = 0; j < 3; j++)
				JY901_data.angle.angle[j] = (float)stcAngle.Angle[j] / 32768 * 180;
			break;

		case 0x54: // 锟脚筹拷锟斤拷锟斤拷
			memcpy(&stcMag, &JY901_data.RxBuffer[2 + i * 11], 8);
			for (uint8_t j = 0; j < 3; j++)
				JY901_data.h.h[j] = (float)stcMag.h[j];
			break;

		case 0x55: // D0-D3锟剿匡拷状态
			break;

		case 0x56: // 锟斤拷压锟竭讹拷
			memcpy(&stcPress, &JY901_data.RxBuffer[2 + i * 11], 8);
			JY901_data.lPressure.lPressure = (float)stcPress.lPressure;
			JY901_data.lPressure.lAltitude = (float)stcPress.lAltitude / 100;
			break;

		case 0x57: // 锟斤拷纬锟斤拷
			memcpy(&stcLonLat.lLat, &JY901_data.RxBuffer[2 + i * 11], 8);
			JY901_data.lLon.lLat = (float)stcLonLat.lLat / 10000000 + (double)(stcLonLat.lLat % 10000000) / 1e5;
			JY901_data.lLon.lLat = (float)stcLonLat.lLon / 10000000 + (double)(stcLonLat.lLon % 10000000) / 1e5;
			break;

		case 0x58: // GPS
			break;

		case 0x59: // 锟斤拷元锟斤拷
			memcpy(&stcQ, &JY901_data.RxBuffer[2 + i * 11], 8);
			for (uint8_t j = 0; j < 4; j++)
				JY901_data.q.q[j] = (float)stcQ.q[j] / 32768;
			break;
		}
	}
}

// 锟斤拷锟捷斤拷锟杰猴拷锟斤拷
void JY901_Rec(void)
{
	/* USER CODE BEGIN USART1_IRQn 0 */
	uint32_t temp_flag = 0;
	uint32_t temp;
	temp_flag = __HAL_UART_GET_FLAG(&USARTHANDLE, UART_FLAG_IDLE);
	if ((temp_flag != RESET))
	{
		__HAL_UART_CLEAR_IDLEFLAG(&USARTHANDLE);
		HAL_UART_DMAStop(&USARTHANDLE);
		// F1锟侥帮拷锟斤拷
		temp = DMAHANDLE.Instance->NDTR;
		JY901_data.Rx_len = RXBUFFER_LEN - temp;
		JY901_Process(); // 锟斤拷锟斤拷锟皆硷拷锟斤拷锟斤拷锟叫达拷锟斤拷锟斤拷锟斤拷
		jy901data.pitch_angle = JY901_data.angle.angle[1];
		jy901data.pitch_w = JY901_data.w.w[1];
		xQueueSendFromISR(JY901B_QueueHandle, &jy901data, pdTRUE);
		JY901_data.Rx_flag = 1;
	}
	HAL_UART_Receive_DMA(&USARTHANDLE, JY901_data.RxBuffer, RXBUFFER_LEN);

	/* USER CODE END USART1_IRQn 0 */
	//   HAL_UART_IRQHandler(&USARTHANDLE);
	/* USER CODE BEGIN USART1_IRQn 1 */

	/* USER CODE END USART1_IRQn 1 */
}

void myprintf(char *fmt, ...)
{
	uint8_t tempbuff[256];
	uint16_t i;
	va_list ap;
	va_start(ap, fmt);
	vsprintf((char *)tempbuff, fmt, ap);
	va_end(ap);

	for (i = 0; i < strlen((char *)tempbuff); i++)
	{
		while (!__HAL_UART_GET_FLAG(&USARTHANDLE, UART_FLAG_TXE))
			;
		USARTHANDLE.Instance->DR = tempbuff[i];
	}
}

// //xy锟斤拷校准锟斤拷锟斤拷
// void Correct_XY(void)
// {
// 	myprintf("%x%x%x%x%x\r\n",0xFF,0xAA,0x69,0x88,0xB5);//锟斤拷锟斤拷
// 	HAL_Delay(500);//锟斤拷时500ms
// 	myprintf("%x%x%x%x%x\r\n",0xFF,0xAA,0x01,0x08,0x00);//锟斤拷锟斤拷锟睫改ｏ拷XY锟斤拷锟斤拷锟姐）
// 	myprintf("%x%x%x%x%x\r\n",0xFF,0xAA,0x00,0x00,0x00);//锟斤拷锟斤拷

// }
